﻿#include "nnom_handlers.h"

#include "elog.h"
#include "mpu6050.h"
#include "nnom.h"
#include "weights.h"

#include "stm32f1xx_hal.h"
#define LOG_TAG "nnom_handlers"
#define SCALE ( pow( 2, INPUT_1_OUTPUT_DEC ) )


#ifdef NNOM_USING_STATIC_MEMORY
uint8_t nnom_buffer[1024 * 8];
#endif

static cnn_obj nnom = { 0 };
static nnom_model_t* model;

void model_feed_data( void )
{
    const double scale = SCALE;
    int i = 0;
    for (i = 0; i < IMU_DATA_INPUT_LEN; i++)
    {
        // nnom_input_data[i*9] = (int8_t)round(IMU_Data_mEulerAngles[i][0] * scale);
        // nnom_input_data[i*9+1] = (int8_t)round(IMU_Data_mEulerAngles[i][1] * scale);
        // nnom_input_data[i*9+2] = (int8_t)round(IMU_Data_mEulerAngles[i][2] * scale);
        //
        // nnom_input_data[i*6] = (int8_t)round(IMU_Data_mAcc[i][0] * scale);
        // nnom_input_data[i*6+1] = (int8_t)round(IMU_Data_mAcc[i][1] * scale);
        // nnom_input_data[i*6+2] = (int8_t)round(IMU_Data_mAcc[i][2] * scale);

        nnom_input_data[i * 3]     = (int8_t)round( nnom.IMU_Data_mGyro[i][IMU_AXIS_X] * scale );
        nnom_input_data[i * 3 + 1] = (int8_t)round( nnom.IMU_Data_mGyro[i][IMU_AXIS_Y] * scale );
        nnom_input_data[i * 3 + 2] = (int8_t)round( nnom.IMU_Data_mGyro[i][IMU_AXIS_Z] * scale );
        //printf("%f %f %f %f %f %f %f %f %f\n", IMU_Data_mAngles[i][0], IMU_Data_mAngles[i][1], IMU_Data_mAngles[i][2], IMU_Data_mGyro[i][0], IMU_Data_mGyro[i][1], IMU_Data_mGyro[i][2], IMU_Data_mAcc[i][0], IMU_Data_mAcc[i][1], IMU_Data_mAcc[i][2]);
    }
}

int8_t model_get_output( void )
{
    uint8_t i = 0;
    int8_t max_output = -128;
    int8_t ret = 0;
    for (i = 0; i < 2; i++)
    {
        log_d( "Output[%d] = %.2f %%", i, ( nnom_output_data[i] / 127.0 ) * 100 );
        if (nnom_output_data[i] >= max_output)
        {
            max_output = nnom_output_data[i];
            ret = (int8_t)i;
        }
    }
    if (max_output >= OUPUT_THRESHOLD)
    {
        log_d( "max_output: %d OUPUT_THRESHOLD: %d", max_output, OUPUT_THRESHOLD );
        return ret;
    }
    return -1;
}

// void model_reasoning( nnom_model_t* model )
// {
//     if ( mpu_model_flag == 1 )
//     {
//         model_feed_data();
//         model_run( model );
//         model_output = model_get_output();
//         //printf("[NNOM_Model] %d \n",model_output);
//         ir_temp = SMBus_ReadTemp();
//         //printf("[ir_temp] %.2lf\n", ir_temp);
//         mpu_model_flag = 0;
//         mpu_model_index = 0;
//
//         switch ( model_output )
//         {
//                 // MOVE
//             case 1:
//                 model_res = 1;
//                 break;
//                 // STOP
//             default:
//                 model_res = 0;
//                 break;
//         }
//         mqtt_gps_model_send_flag = 1;
//         //printf("[NNOM_Model] res = %s\n", model_res);
//     }
// }

void nnom_run( void )
{
    nnom.status = NN_STATUS_INFER_NOT_DONE;
    if (nnom.IMU_Data_Index == IMU_DATA_INPUT_LEN)
    {
        HAL_NVIC_DisableIRQ( EXTI9_5_IRQn );
        log_d( "DisableIRQ EXTI9_5_IRQn" );
        model_feed_data();
        model_run( model );

        int8_t model_output = model_get_output();

        switch (model_output)
        {
            // MOVE
            case 1:
                nnom.cnn_result = model_output;
                log_i( "MOVE" );
                break;
            // STOP
            default:
                nnom.cnn_result = 0;
                log_i( "STOP" );
                break;
        }

        nnom.IMU_Data_Index = 0;
        nnom.status = NN_STATUS_INFER_DONE;
        log_d( "EnableIRQ EXTI9_5_IRQn" );
        HAL_NVIC_EnableIRQ( EXTI9_5_IRQn );
    }

    if (nnom.mpu6050->data_flag == 1)
    {
        nnom.IMU_Data_mGyro[nnom.IMU_Data_Index][IMU_AXIS_X] = nnom.mpu6050->accel_x;
        nnom.IMU_Data_mGyro[nnom.IMU_Data_Index][IMU_AXIS_Y] = nnom.mpu6050->accel_y;
        nnom.IMU_Data_mGyro[nnom.IMU_Data_Index][IMU_AXIS_Z] = nnom.mpu6050->accel_z;

        nnom.IMU_Data_Index++;

        // log_d( "IMU_AXIS_X: %f IMU_AXIS_Y: %f IMU_AXIS_X: %Z ",
        //       nnom.IMU_Data_mGyro[nnom.IMU_Data_Index][IMU_AXIS_X],
        //       nnom.IMU_Data_mGyro[nnom.IMU_Data_Index][IMU_AXIS_Y],
        //       nnom.IMU_Data_mGyro[nnom.IMU_Data_Index][IMU_AXIS_Z] );
        //
        // log_d( "pitch: %f roll: %f yaw: %f", nnom.mpu6050->pitch, nnom.mpu6050->roll, nnom.mpu6050->yaw );
        //
        // log_d( "IMU_Data_Index : %d", nnom.IMU_Data_Index );
        nnom.mpu6050->data_flag = 0;
    }
}

cnn_obj* create_nnom_Obj( void )
{
    if (!nnom.status)
    {
        nnom.status = NN_STATUS_INITIALIZED;

#ifdef NNOM_USING_STATIC_MEMORY
        nnom_set_static_buf( nnom_buffer, sizeof( nnom_buffer ) );
#endif
        model = nnom_model_create();
        log_d( "NNOM Create" );
        nnom.mpu6050 = create_MPU6050_Obj();
        log_d( "MPU6050 Create" );
        nnom.run = nnom_run;
    }

    return &nnom;
}